% NMPC 控制四旋翼无人机 - 轨迹跟踪
clear;
clc;

% 物理参数
m = 1.0;        % 无人机质量 (kg)
g = 9.81;       % 重力加速度 (m/s^2)
l = 0.25;       % 四旋翼臂长 (m)
Ixx = 0.01;     % 无人机的惯性矩 (kg.m^2)
Iyy = 0.01;
Izz = 0.02;
k = 0.1;        % 推力常数
b = 0.01;       % 阻力常数

% 状态变量 [x, y, z, vx, vy, vz, roll, pitch, yaw, p, q, r]
% 状态向量：x, y, z 为位置，vx, vy, vz 为速度，roll, pitch, yaw 为姿态，p, q, r 为角速度

% 轨迹生成 (圆形轨迹)
t_total = 20; % 总时间
dt = 0.1; % 时间步长
time = 0:dt:t_total;
radius = 5; % 圆轨迹半径
z_height = 2; % 轨迹的高度
freq = 0.1; % 轨迹旋转频率

x_ref = radius * cos(2 * pi * freq * time);
y_ref = radius * sin(2 * pi * freq * time);
z_ref = z_height * ones(size(time)); % 保持恒定高度

% 控制器设置
N = 20;            % 预测时域长度
x_current = [0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0; 0]; % 初始状态
u_current = [0; 0; 0; 0]; % 初始控制输入

% 系统动态模型
f = @(x, u) [
    x(4);              % 位置变化率
    x(5);              % 位置变化率
    x(6);              % 位置变化率
    (u(1) * cos(x(7)) * sin(x(8)) + u(2) * cos(x(7)) * cos(x(8)) - m * g) / m; % x方向加速度
    (u(1) * sin(x(7)) * sin(x(8)) + u(2) * sin(x(7)) * cos(x(8))) / m; % y方向加速度
    (u(1) * cos(x(8)) + u(2) * sin(x(8))) / m; % z方向加速度
    x(10) + (x(11) * sin(x(7)) + x(12) * cos(x(7))) / cos(x(8)); % roll角加速度
    x(11) + (x(10) * cos(x(7)) - x(12) * sin(x(7))); % pitch角加速度
    x(12) + (x(10) * sin(x(7)) + x(11) * cos(x(7))); % yaw角加速度
    (u(3) - u(4)) / Ixx; % p角速度变化
    (u(4) - u(3)) / Iyy; % q角速度变化
    (u(3) - u(4)) / Izz  % r角速度变化
];

% 目标函数和约束（NMPC优化问题）
options = optimoptions('fmincon', 'Display', 'off', 'Algorithm', 'sqp');

% 轨迹跟踪仿真
state_history = zeros(12, length(time));
control_history = zeros(4, length(time));

for i = 1:length(time)
    % 更新目标点为轨迹上的下一个位置
    x_target = [x_ref(i); y_ref(i); z_ref(i)];
    
    % 计算当前状态误差
    x_error = x_target - x_current(1:3); % 位置误差
    
    % 设置一个优化问题来最小化目标函数
    cost_function = @(u) (norm(x_error)^2 + norm(u)^2); % 目标函数，结合位置误差和控制输入的平方和
    
    % 约束条件 (例如控制输入范围)
    lb = [-10; -10; -10; -10]; % 控制输入下限
    ub = [10; 10; 10; 10];     % 控制输入上限
    
    % 使用fmincon求解NMPC控制问题
    u_opt = fmincon(cost_function, u_current, [], [], [], [], lb, ub, [], options);
    
    % 更新控制输入
    u_current = u_opt;
    
    % 使用控制输入更新状态
    x_current = x_current + dt * f(x_current, u_current);
    
    % 记录状态和控制输入
    state_history(:, i) = x_current;
    control_history(:, i) = u_current;
end

% 绘图
figure;

% 轨迹与位置对比
subplot(3,1,1);
plot3(x_ref, y_ref, z_ref, 'r--', 'LineWidth', 1.5); hold on;
plot3(state_history(1,:), state_history(2,:), state_history(3,:), 'b', 'LineWidth', 1.5);
legend('Reference Trajectory', 'Actual Path');
xlabel('X');
ylabel('Y');
zlabel('Z');
title('3D Trajectory Tracking');
grid on;

% 位置跟踪误差
subplot(3,1,2);
plot(time, x_ref - state_history(1,:), 'r', 'LineWidth', 1.5); hold on;
plot(time, y_ref - state_history(2,:), 'g', 'LineWidth', 1.5);
plot(time, z_ref - state_history(3,:), 'b', 'LineWidth', 1.5);
legend('Error in X', 'Error in Y', 'Error in Z');
xlabel('Time (s)');
ylabel('Tracking Error');
title('Position Tracking Error');

% 控制输入
subplot(3,1,3);
plot(time, control_history(1,:), 'r', 'LineWidth', 1.5); hold on;
plot(time, control_history(2,:), 'g', 'LineWidth', 1.5);
plot(time, control_history(3,:), 'b', 'LineWidth', 1.5);
plot(time, control_history(4,:), 'k', 'LineWidth', 1.5);
legend('u1', 'u2', 'u3', 'u4');
xlabel('Time (s)');
ylabel('Control Inputs');
title('Control Inputs over Time');
